package org.maxgroundstation.simulation;

import java.io.IOException;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import java.net.SocketException;
import java.util.HashMap;
import java.util.Map;

/**
 * A receiver for UDP network communication commands.
 * @author Scott Davis
 * 
 * Copyright (C) 2010  Scott Davis
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
public class UdpCommandReceiver implements Runnable {
    
	// Length of server thread sleep time (milliseconds).
    private static final long SLEEP_TIME = 20L;
	
    // Default communication port for the robot.
    private static final int DEFAULT_PORT = 10301;
    
    // MAX robot UDP command types
    public static final int MOTOR_COMMAND = 2;
    public static final int TURN_COMMAND = 3;
    public static final int TURN_FRONT_COMMAND = 4;
    public static final int TURN_REAR_COMMAND = 5;
    public static final int PAN_COMMAND = 6;
    public static final int TILT_COMMAND = 7;
    public static final int IM_ALIVE_COMMAND = 0;
    public static final int TEXT_COMMAND = 1;
    
    // Data members
    private MaxRobot robot;
    private DatagramSocket socket;
    private Thread serverThread;
    private Map<Integer, String> commandMap;
    private String lastDatagramAddress;
    private int currentCommandIterator;
    private boolean run;
    
    /**
     * Constructor
     * @param robot the MAX robot simulation.
     */
    UdpCommandReceiver(MaxRobot robot) {
        this.robot = robot;
        currentCommandIterator = 0;
        
        // Initialize command map.
        commandMap = new HashMap<Integer, String>(6);
        commandMap.put(MOTOR_COMMAND, "motor");
        commandMap.put(TURN_COMMAND, "turn");
        commandMap.put(TURN_FRONT_COMMAND, "turnfront");
        commandMap.put(TURN_REAR_COMMAND, "turnrear");
        commandMap.put(PAN_COMMAND, "pan");
        commandMap.put(TILT_COMMAND, "tilt");
        commandMap.put(IM_ALIVE_COMMAND, "imalive");
        
        try {
            InetAddress localAddress = MaxRobotUtil.getLocalAddress();
            
            socket = new DatagramSocket(DEFAULT_PORT, localAddress);
            System.out.println("MAX robot sim host: " + socket.getLocalAddress() + 
                    " on port: " + socket.getLocalPort());
        }
        catch (IOException e) {
            System.err.println("Error creating datagram socket: " + e.getMessage());
        }
        
        start();
    }
    
    /**
     * Starts the server thread.
     */
    private void start() {
        if ((serverThread == null) || !run) { 
            serverThread = new Thread(this, "UPD receiver server thread");
            serverThread.start();
        }
    }
    
    /**
     * Run the server thread.
     */
    public void run() {
        run = true;
        while (run) {
        	try {
                Thread.sleep(SLEEP_TIME);
            } 
            catch (InterruptedException e) {}
        	
            try {
                byte[] buf = new byte[1000];
                DatagramPacket packet = new DatagramPacket(buf, buf.length);
                socket.receive(packet);
                lastDatagramAddress = packet.getAddress().getHostAddress();
                
                byte[] data = packet.getData();
                String commandString = new String(data);
                //System.out.println("Receiving UDP command: " + commandString);
                sendCommand(commandString);
            }
            catch (SocketException e) {
                run = false;
            }
            catch (Exception e) {
                System.out.println("Error running server thread: " + e.getMessage());
                e.printStackTrace(System.err);
            }
        }
    }
    
    /**
     * Close the datagram socket.
     */
    void closeSocket() {
        if (socket != null) socket.close();
    }
    
    /**
     * Gets the local host IP address.
     * @return IP address.
     */
    InetAddress getHostIP() {
        InetAddress result = null;
        if (socket != null) result = socket.getLocalAddress();
        return result;
    }
    
    /**
     * Gets the port number.
     * @return port number.
     */
    int getPort() {
        int result = 0;
        if (socket != null) result = socket.getLocalPort();
        return result;
    }
    
    /**
     * Gets the IP address of the last datagram packet received.
     * @return IP address string or null if none.
     */
    public String getLastDatagramAddress() {
        return lastDatagramAddress;
    }
    
    /**
     * Sends a command to the MAX robot simulation.
     * @param commandString the encoded command string.
     * @throws Exception if error in command string.
     */
    private void sendCommand(String commandString) throws Exception {
        
        if (commandString == null) 
            throw new IllegalArgumentException("commandString cannot be null");
        if (commandString.trim().length() == 0) 
            throw new IllegalArgumentException("commandString cannot be empty");
        
        // Parse command string.
        int commandType = Integer.parseInt(commandString.substring(0, 1));
        
        // Ignore command iterator for now.
        int commandIterator = Integer.parseInt(commandString.substring(1, 3));
        if (commandType == IM_ALIVE_COMMAND) currentCommandIterator = commandIterator;
        else if (commandIterator != currentCommandIterator) 
            throw new Exception("Command out of sequence!");
        
        String commandName = null;
        if (commandMap.containsKey(commandType)) {
            commandName = commandMap.get(commandType);
            
            float value = Float.parseFloat(commandString.substring(4));
            
            //System.out.println("Performing robot command: " + commandName + "(" + value + ")");
            robot.sendCommand(commandName + "(" + value + ")");
        }
        else if (commandType == TEXT_COMMAND) {
            String textCommand = commandString.substring(4).trim();
            robot.sendTextCommand(textCommand);
        }
    }
}